The goal of this lab was to localize a virtual robot in a known map using a Bayes filter. The robot state is \((x, y, \theta)\), and the simulator discretizes the continuous world into a \(12 \times 9 \times 18\) grid. Instead of relying only on odometry, which accumulates error over time, the Bayes filter combines a motion model with sensor observations to estimate the robot pose more accurately.
The filter has two steps that run every time the robot moves. First is the prediction step, which takes the control input and updates the belief based on where the robot probably moved to:
Then the update step uses the new sensor reading to correct that prediction, which brings the error back down:
I used the odometry model to break down the motion between two poses into three pieces: a first rotation, a straight translation, and a second rotation.
The robot does a full 360° spin at each step and collects 18 range readings spaced 20° apart. I treated each beam as independent and modeled the likelihood of each one as a Gaussian centered on the precomputed true range for that cell:
The main work was filling in five functions in the notebook and here's what I did for each one.
compute_control() takes two poses and pulls out the (rot1, trans, rot2) values the motion model needs. One thing I had to be careful about was when delta_trans is basically zero. In that case, the robot only rotated in place and atan2(0, 0) is undefined, so I set rot1 to 0. I also wrap all the angles back to \([-180, 180)\) before returning, otherwise the Gaussians get evaluated with the wrong angle difference.
def compute_control(cur_pose, prev_pose):
dx = cur_pose[0] - prev_pose[0]
dy = cur_pose[1] - prev_pose[1]
delta_trans = np.hypot(dx, dy)
if delta_trans < EPS:
delta_rot_1 = 0.0
else:
delta_rot_1 = wrap_angle(np.degrees(np.arctan2(dy, dx)) - prev_pose[2])
delta_rot_2 = wrap_angle(cur_pose[2] - prev_pose[2] - delta_rot_1)
return float(delta_rot_1), float(delta_trans), float(delta_rot_2)
odom_motion_model() computes \(p(x_t \mid u_t, x_{t-1})\). For a given previous and current pose pair, I use compute_control() to get what the ideal motion would have been for that transition. Then I compare it to the actual measured odometry u using three Gaussians: one for rot1, one for trans, and one for rot2. Multiplying them together gives the transition probability.
def odom_motion_model(cur_pose, prev_pose, u):
rot1_hat, trans_hat, rot2_hat = compute_control(cur_pose, prev_pose)
rot1, trans, rot2 = u
prob = (
loc.gaussian(rot1, rot1_hat, loc.odom_rot_sigma)
* loc.gaussian(trans, trans_hat, loc.odom_trans_sigma)
* loc.gaussian(rot2, rot2_hat, loc.odom_rot_sigma)
)
return float(prob)
prediction_step() is the slow one. In theory it's a double loop over all 1,944 previous states and all 1,944 current states, which is almost 4 million iterations. To speed it up I skip any previous state with belief below 1e-4 since those contribute almost nothing. Then for each current state I vectorize the computation across all the active previous cells at once using NumPy instead of looping through them one by one. That made it fast enough to actually run in a reasonable amount of time.
def prediction_step(cur_odom, prev_odom):
actual_u = compute_control(cur_odom, prev_odom)
loc.bel_bar.fill(0.0)
active = loc.bel > BELIEF_FLOOR
if not np.any(active):
active = loc.bel > 0
prev_bel = loc.bel[active]
prev_x = mapper.x_values[active]
prev_y = mapper.y_values[active]
prev_a = mapper.a_values[active]
for cx in range(mapper.MAX_CELLS_X):
for cy in range(mapper.MAX_CELLS_Y):
for ca in range(mapper.MAX_CELLS_A):
cur_x, cur_y, cur_a = mapper.from_map(cx, cy, ca)
dx = cur_x - prev_x
dy = cur_y - prev_y
trans_hat = np.hypot(dx, dy)
rot1_hat = wrap_angle(np.degrees(np.arctan2(dy, dx)) - prev_a)
rot1_hat = np.where(trans_hat < EPS, 0.0, rot1_hat)
rot2_hat = wrap_angle(cur_a - prev_a - rot1_hat)
transition_prob = (
loc.gaussian(actual_u[0], rot1_hat, loc.odom_rot_sigma)
* loc.gaussian(actual_u[1], trans_hat, loc.odom_trans_sigma)
* loc.gaussian(actual_u[2], rot2_hat, loc.odom_rot_sigma)
)
loc.bel_bar[cx, cy, ca] = np.sum(prev_bel * transition_prob)
loc.bel_bar /= np.sum(loc.bel_bar)
sensor_model() just returns the per-beam likelihood array for one candidate cell, comparing the actual scan in loc.obs_range_data against the precomputed expected values for that cell.
def sensor_model(obs):
expected_obs = np.asarray(obs).reshape(-1)
actual_obs = np.asarray(loc.obs_range_data).reshape(-1)
return loc.gaussian(actual_obs, expected_obs, loc.sensor_sigma)
I skipped the loop entirely for update_step(). Since mapper.obs_views already has the precomputed true observations for every cell stored as a (12, 9, 18, 18) array, I just reshaped actual_obs to (1, 1, 1, 18) and let NumPy broadcast the math across all cells at once. I also did everything in log-space and subtracted the max before exponentiating, which stops underflow from happening when you multiply 18 small numbers together.
def update_step():
actual_obs = np.asarray(loc.obs_range_data).reshape(1, 1, 1, -1)
expected_obs = mapper.obs_views # shape (12, 9, 18, 18)
sq_error = (actual_obs - expected_obs) ** 2
log_likelihood = -np.sum(sq_error / (2 * loc.sensor_sigma ** 2), axis=3)
log_likelihood -= np.max(log_likelihood)
likelihood = np.exp(log_likelihood)
loc.bel = loc.bel_bar * likelihood
loc.bel /= np.sum(loc.bel)I first ran the trajectory with just raw odometry and no filter to see how bad the drift was. The red path starts near the ground truth but falls apart almost right away, and by the end it has gone well outside the map.
Next, I ran the observation loop by itself. During this step, the robot rotated in place and scanned the room before moving again. This is expected because each Bayes filter update requires a full 18-beam scan.
Finally, I ran the full localization loop. In the final plot, the blue belief trajectory stayed much closer to the green ground truth than the red odometry trajectory. The blue path is somewhat blocky because the estimate is constrained to discrete grid cells.
It worked best near walls and corners where the scan is distinctive enough to rule out most of the grid. In the more open parts of the map the neighboring cells look too similar to each other, so the belief spread a bit more there. After big rotation steps the prediction also spread out a lot and needed the next scan to recover. But overall it held up really well compared to just using odometry.
The table below shows the most probable state after each update step of the Bayes filter, along with its probability and the error compared to the ground truth pose.
| Iteration | GT index | Most probable index | Probability | XY error (m) | Heading error (deg) |
|---|---|---|---|---|---|
| Initial update | (6,4,9) | (5,4,9) | 0.999956 | 0.000 | -10.000 |
| t=0 | (6,3,7) | (6,4,6) | 1.000000 | 0.089 | 10.943 |
| t=1 | (7,2,5) | (6,2,5) | 1.000000 | 0.230 | 8.025 |
| t=2 | (7,2,4) | (6,2,4) | 1.000000 | 0.230 | 5.487 |
| t=3 | (7,1,4) | (7,1,4) | 1.000000 | 0.059 | 5.487 |
| t=4 | (8,0,8) | (6,1,8) | 1.000000 | 0.524 | 9.997 |
| t=5 | (11,1,11) | (10,1,11) | 1.000000 | 0.070 | -1.542 |
| t=6 | (11,2,12) | (11,3,13) | 1.000000 | 0.250 | -13.366 |
| t=7 | (11,3,13) | (11,3,13) | 1.000000 | 0.162 | -7.730 |
| t=8 | (11,5,14) | (11,4,13) | 0.999999 | 0.338 | 14.620 |
| t=9 | (11,6,16) | (11,7,16) | 1.000000 | 0.257 | -7.187 |
| t=10 | (10,7,16) | (10,7,16) | 1.000000 | 0.127 | 4.083 |
| t=11 | (7,7,3) | (7,7,3) | 1.000000 | 0.090 | -0.155 |
| t=12 | (6,5,5) | (6,5,5) | 1.000000 | 0.011 | 4.927 |
| t=13 | (6,4,2) | (6,3,14) | 0.923301 | 0.428 | 117.330 |
| t=14 | (4,3,1) | (4,3,1) | 1.000000 | 0.201 | -5.211 |
| t=15 | (3,3,0) | (3,3,0) | 0.564092 | 0.254 | -7.752 |
From the table, the most probable state after the update step is often either the exact ground truth cell or a neighboring cell with small position error. In most iterations the filter assigns very high confidence, showing that the observation model is strong. The main exception is t=13, where the filter selects an incorrect nearby state with a much larger heading error. This likely happened because multiple states produced similar scan patterns in that part of the map. The filter was still able to recover in later iterations when the observations became more distinctive.
This lab was a good reminder of how quickly odometry fails even in simulation with no real-world noise. It was good to see the lecture material actually work when I ran it, and I feel a lot more ready for doing this on the real robot in Lab 11.